﻿using System;

namespace System
{
	public struct B_Matrix
	{
		public double[,] Data;
		public int NRow;
		public int NCol;
		//Constructors & Operators
		public B_Matrix Clone()
		{
			B_Matrix A = new B_Matrix(NRow, NCol, this.Data);
			return A;
		}
		public B_Matrix(int row, int col)
		{
			NRow = row;
			NCol = col;
			Data = new double[row, col];
		}
		public B_Matrix(int n)
			: this(n, n)
		{
		}
		public B_Matrix(int row, int col, double[,] values)
			: this(row, col)
		{
			Buffer.BlockCopy(values, 0, Data, 0, NRow * NCol * 8);
		}

		public B_Matrix(B_Matrix Base)
			: this(Base.NRow, Base.NCol)
		{
			Buffer.BlockCopy(Base.Data, 0, Data, 0, NRow * NCol * 8);
		}
		public static B_Matrix operator +(B_Matrix m1, B_Matrix m2)
		{
			B_Matrix Res = new B_Matrix(m1.NRow, m1.NCol);
			for (int i = 0; i < m1.NRow; i++)
				for (int j = 0; j < m1.NCol; j++)
					Res[i, j] = m1[i, j] + m2[i, j];
			return Res;
		}
		public static B_Matrix operator -(B_Matrix m1, B_Matrix m2)
		{
			B_Matrix Res = new B_Matrix(m1.NRow, m1.NCol);
			for (int i = 0; i < m1.NRow; i++)
				for (int j = 0; j < m1.NCol; j++)
					Res[i, j] = m1[i, j] + m2[i, j];
			return Res;
		}
		public static B_Matrix E(int n, int j)
		{
			B_Matrix e = new B_Matrix(n, 1);
			e[j, 0] = 1;
			return e;
		}
		public static B_Matrix Identity(int n)
		{
			B_Matrix Res = new B_Matrix(n, n);
			for (int i = 0; i < n; i++)
				Res[i, i] = 1;
			return Res;
		}

		//Data Fetch
		public double this[int row, int col]
		{
			get
			{
				return Data[row, col];
			}
			set
			{
				Data[row, col] = value;
			}
		}
		public double[] GetRow(int n)
		{
			double[] Res = new double[NCol];
			for (int i = 0; i < NCol; i++)
				Res[i] = this[n, i];
			return Res;
		}
		public double[] GetCol(int n)
		{
			double[] Res = new double[NRow];
			for (int i = 0; i < NRow; i++)
				Res[i] = this[i, n];
			return Res;
		}

		void SetRow(int N, double[] val)
		{
			for (int i = 0; i < NCol; i++)
				Data[N, i] = val[i];
		}
		void SetCol(int N, double[] val)
		{
			for (int i = 0; i < NRow; i++)
				Data[i, N] = val[i];
		}
		public double[] ToArray()
		{
			double[] Res = new double[NCol * NRow];
			for (int i = 0; i < NRow; i++)
				for (int j = 0; j < NCol; j++)
					Res[i * NCol + j] = Data[i, j];
			return Res;
		}

		public static B_Matrix FromArray(double[] data, int nrow, int ncol)
		{
			if (data.Length == nrow * ncol)
			{
				B_Matrix Res = new B_Matrix(nrow, ncol);
				for (int i = 0; i < nrow; i++)
					for (int j = 0; j < ncol; j++)
						Res[i, j] = data[ncol * i + j];
				return Res;
			}
			else
				throw new Exception("invalid matrix input");
		}
		//Concat

		//Dynamic funcs
		public B_Matrix ExtractLowerTrapeze()
		{
			B_Matrix buf = new B_Matrix(NRow, NCol);

			for (int i = 1; i <= NRow; i++)
			{
				for (int j = 1; j <= i; j++)
				{
					buf[i, j] = this[i, j];
				}
			}

			return buf;
		}
		public B_Matrix ExtractUpperTrapeze()
		{
			B_Matrix buf = new B_Matrix(NRow, NCol);

			for (int i = 1; i <= NRow; i++)
			{
				for (int j = i; j <= NCol; j++)
				{
					buf[i, j] = this[i, j];
				}
			}

			return buf;
		}
		public B_Matrix[] ColumnVectorize()
		{
			B_Matrix[] buf = new B_Matrix[NCol];

			for (int j = 1; j <= buf.Length; j++)
			{
				buf[j] = this.Column(j);
			}

			return buf;
		}
		public B_Matrix[] RowVectorize()
		{
			B_Matrix[] buf = new B_Matrix[NRow];

			for (int i = 1; i <= buf.Length; i++)
			{
				buf[i] = this.Row(i);
			}

			return buf;
		}
		public void _VerticalFlip()
		{
		}
		public void _HorizontalFlip()
		{
		}
		public void _SwapColumns(int j1, int j2)
		{
		}
		public void SwapRows(int i1, int i2)
		{
			for (int i = 0; i < NCol; i++)
				Algebra.Swap(ref Data[i1, i], ref Data[i2, i]);
		}
		public void MultipleRow(int r, double q)
		{
			for (int i = 0; i < NCol; i++)
				Data[r, i] *= q;
		}
		public void _DeleteRow(int i)
		{
		}
		public void _DeleteColumn(int j)
		{
		}
		public B_Matrix _ExtractRow(int i)
		{
			throw new NotImplementedException();
		}
		public B_Matrix ExtractColumn(int j)
		{
			throw new NotImplementedException();
		}
		void _InsertRow(B_Matrix row, int i)
		{
		}
		void Insert(int i, int j, B_Matrix M)
		{
			for (int m = 1; m <= M.NRow; m++)
				for (int n = 1; n <= M.NCol; n++)
					this[i + m - 1, j + n - 1] = M[m, n];
		}
		public void _InsertColumn(B_Matrix col, int j)
		{
		}
		public B_Matrix Inverse()
		{
			if (NCol != NRow)
				throw new NotImplementedException("Rows and columns are differing!");
			else
			{
				B_Matrix w = new B_Matrix(NCol, NCol * 2);
				for (int i = 0; i < NCol; i++)
					for (int j = 0; j < NCol; j++)
					{
						w[i, j] = this[i, j];
						w[i, j + NCol] = i == j ? 1 : 0;
					}
				w.MakeEuler();
				B_Matrix res = new B_Matrix(NCol);
				for (int i = 0; i < NCol; i++)
					for (int j = 0; j < NCol; j++)
						res[i, j] = w[i, j + NCol];
				return res;
			}
		}
		public void MakeEuler()
		{
			//Check if axis contains 0)
			for (int rowid = 0; rowid < NRow; rowid++)
				if (Data[rowid, rowid] == 0)
				{
					throw new Exception("0 found in axis");
				}
			for (int rowid = 0; rowid < NRow; rowid++)
			{
				double A = Data[rowid, rowid];
				for (int i = 0; i < NRow; i++)
					if (i != rowid && Data[i, rowid] != 0)
					{
						double q = Data[i, rowid] / A;
						for (int j = 0; j < NCol; j++)
							Data[i, j] -= Data[rowid, j] * q;
					}
				MultipleRow(rowid, 1 / A);
			}
		}
		public B_Matrix _Minor(int i, int j)
		{
			B_Matrix A = this.Clone();
			A._DeleteRow(i);
			A._DeleteColumn(j);
			return A;
		}
		/// <summary>
		/// Retrieves column with one-based index j.
		/// </summary>
		/// <param name="j"></param>
		/// <returns>j-th column...</returns>
		public B_Matrix Column(int C)
		{
			B_Matrix buf = new B_Matrix(this.NRow, 1);
			for (int i = 0; i < this.NRow; i++)
			{
				buf[i, 0] = this[i, C];
			}
			return buf;
		}
		/// <summary>
		/// Retrieves row with one-based index i.
		/// </summary>
		/// <param name="i"></param>
		/// <returns>i-th row...</returns>
		public B_Matrix Row(int R)
		{
			B_Matrix buf = new B_Matrix(1, this.NCol);
			for (int i = 0; i < this.NCol; i++)
			{
				buf[0, i] = this[R, i];
			}
			return buf;
		}

		public B_Matrix Transpose()
		{
			B_Matrix M = new B_Matrix(NCol, NRow);

			for (int i = 0; i < NCol; i++)
				for (int j = 0; j < NRow; j++)
					M[i, j] = this[j, i];
			return M;
		}

		//Math
		public static double Dot(B_Matrix v, B_Matrix w)
		{
			int m = v.VectorLength();
			int n = w.VectorLength();

			if (m == 0 || n == 0)
				throw new ArgumentException("Arguments need to be vectors.");
			else if (m != n)
				throw new ArgumentException("Vectors must be of the same length.");

			double buf = 0;

			for (int i = 0; i < m; i++)
			{
				buf += v[0, i] * w[i, 0];
			}

			return buf;
		}
		public static double Fib(int n)
		{
			B_Matrix M = new B_Matrix(2);
			M[0, 0] = M[0, 1] = M[1, 0] = 1;
			M[1, 1] = 0;

			return (M ^ (n - 1))[0, 0];
		}
		/// <summary>
		/// Calcs determinant of square matrix
		/// </summary>
		/// <returns></returns>
		public double _Determinant()
		{
			B_Matrix M = this.Clone();
			for (int i = 0; i < NRow; i++)
			{
				double D = M[i, i];
				{
					for (int j = 0; j < NRow; j++)
						if (i != j)
						{
							double D2 = M[j, i];

							for (int k = 0; k < NCol; k++)
								M[j, k] -= D2 / D * M[i, k];
						}
				}
			}

			for (int i = 0; i < NCol; i++)
				for (int j = 0; j < NRow; j++)
				{
					Console.SetCursorPosition(j * 8, i);
					Console.Write(M[i, j].ToString("0.000"));
				}
			return -1;
		}
		/// <summary>
		/// Computes signum of a permutation matrix, which is 1 for an even
		/// number of swaps and -1 for an odd number of swaps. WARNING: 
		/// if *this is not a permutation matrix (e.i. a permutation of Id),
		/// garbage is returned.
		/// </summary>
		/// <returns></returns>
		public double Signum()
		{
			double buf = 1;

			int n = NRow;
			double fi, fj;

			for (int i = 1; i < n; i++)
			{
				for (fi = 1; fi < n && this[i, (int)fi] != 1; fi++)
					;

				for (int j = i + 1; j <= n; j++)
				{
					for (fj = 1; fj <= n && this[j, (int)fj] != 1; fj++)
						;

					buf *= (fi - fj) / (i - j);
				}
			}

			return buf;
		}
		public double ColumnSum(int j)
		{
			if (j <= 0 || j > NCol)
				throw new ArgumentException("Index out of range.");
			double buf = 0;
			for (int i = 0; i < NRow; i++)
			{
				buf += this[i, j];
			}
			return buf;
		}
		public double RowSum(int i)
		{
			if (i <= 0 || i > NCol)
				throw new ArgumentException("Index out of range.");
			double buf = 0;
			for (int j = 0; j < NCol; j++)
			{
				buf += this[i, j];
			}
			return buf;
		}
		/// <summary>
		/// Calcs trace of the matrix.
		/// </summary>
		/// <returns>Sum of diagonal elements.</returns>
		public double Trace()
		{
			if (!this.IsSquare())
				throw new InvalidOperationException("Cannot calc trace of non-square matrix.");

			double buf = 0;

			for (int i = 1; i <= this.NRow; i++)
			{
				buf += this[i, i];
			}

			return buf;
		}


		//Checks

		/// <summary>
		/// Checks if matrix is n by one or one by n.
		/// </summary>
		/// <returns>Length, if vector; zero else.</returns>
		public int VectorLength()
		{
			if (NCol > 1 && NRow > 1)
				return 0;
			else
				return Math.Max(NCol, NRow);
		}
		/// <summary>
		/// Checks if number of rows equals number of columns.
		/// </summary>
		/// <returns>True iff matrix is n by n.</returns>
		public bool IsSquare()
		{
			return (this.NCol == this.NRow);
		}

		/// <summary>
		/// Checks if matrix is involutary, e.i. if A*A = id.
		/// </summary>
		/// <returns>True iff matrix is involutary.</returns>
		public bool IsInvolutary()
		{
			return (this * this == Identity(NRow));
		}

		/// <summary>
		/// Checks if A[i, j] == A[j, i].
		/// </summary>
		/// <returns>True iff matrix is symmetric.</returns>
		public bool IsSymmetric()
		{
			for (int i = 1; i <= this.NRow; i++)
			{
				for (int j = 1; j <= this.NCol; j++)
				{
					if (this[i, j] != this[j, i])
						return false;
				}
			}

			return true;
		}

		/// <summary>
		/// Checks for orthogonality by testing if A*A' == id.
		/// </summary>
		/// <returns>True iff matrix is orthogonal.</returns>
		public bool IsOrthogonal()
		{
			return (this.IsSquare() && this * this.Transpose() == Identity(this.NRow));
		}

		/// <summary>
		/// Checks if matrix is lower or upper trapeze.
		/// </summary>
		/// <returns>True iff matrix is trapeze.</returns>
		public bool IsTrapeze()
		{
			return (this.IsUpperTrapeze() || this.IsLowerTrapeze());
		}
		/// <summary>
		/// Checks if matrix is trapeze and square.
		/// </summary>
		/// <returns>True iff matrix is triangular.</returns>
		public bool IsTriangular()
		{
			return (this.IsLowerTriangular() || this.IsUpperTriangular());
		}
		/// <summary>
		/// Checks if matrix is square and upper trapeze.
		/// </summary>
		/// <returns>True iff matrix is upper triangular.</returns>
		public bool IsUpperTriangular()
		{
			return (this.IsSquare() && this.IsUpperTrapeze());
		}
		/// <summary>
		/// Checks if matrix is square and lower trapeze.
		/// </summary>
		/// <returns>True iff matrix is lower triangular.</returns>
		public bool IsLowerTriangular()
		{
			return (this.IsSquare() && this.IsLowerTrapeze());
		}
		/// <summary>
		/// Checks if A[i, j] == 0 for i < j.
		/// </summary>
		/// <returns>True iff matrix is upper trapeze.</returns>
		public bool IsUpperTrapeze()
		{
			for (int j = 1; j <= NCol; j++)
				for (int i = j + 1; i <= NRow; i++)
					if (this[i, j] != 0)
						return false;

			return true;
		}
		/// <summary>
		/// Checks if A[i, j] == 0 for i > j.
		/// </summary>
		/// <returns>True iff matrix is lower trapeze.</returns>
		public bool IsLowerTrapeze()
		{
			for (int i = 1; i <= NRow; i++)
				for (int j = i + 1; j <= NCol; j++)
					if (this[i, j] != 0)
						return false;

			return true;
		}

		//Overrides & Operators
		public override String ToString()
		{
			string s = "";
			double buf;
			for (int i = 0; i < NRow; i++)
			{
				for (int j = 0; j < NCol; j++)
				{
					buf = this[i, j];
					s += buf.ToString();
					s += ";" + "\t";
				}
				s += "\\" + System.Environment.NewLine;
			}
			return s;
		}
		public string ToString(string format)
		{
			string s = "";
			double buf;
			for (int i = 1; i <= NRow; i++)
			{
				for (int j = 1; j <= NCol; j++)
				{
					buf = this[i, j];
					s += buf.ToString(format);
					s += ";" + "\t";
				}
				s += "\\" + System.Environment.NewLine;
			}
			return s;
		}
		public override bool Equals(object obj)
		{
			return obj.ToString() == this.ToString();
		}
		public override int GetHashCode()
		{
			return -1;
		}
		public static bool operator ==(B_Matrix A, B_Matrix B)
		{
			if (A.NCol != B.NCol)
				return false;
			if (A.NRow != B.NRow)
				return false;
			for (int i = 0; i < A.NRow; i++)
				for (int j = 0; j < A.NCol; j++)
					if (A[i, j] != B[i, j])
						return false;
			return true;
		}
		public static bool operator !=(B_Matrix A, B_Matrix B)
		{
			return !(A == B);
		}
		public static B_Matrix operator -(B_Matrix A)
		{

			for (int i = 1; i <= A.NRow; i++)
			{
				for (int j = 1; j <= A.NCol; j++)
				{
					A[i, j] = -A[i, j];
				}
			}

			return A;
		}
		public static B_Matrix operator *(B_Matrix A, B_Matrix B)
		{
			if (A.NCol!= B.NRow)
				throw new ArgumentException("Inner matrix dimensions must agree.");
			B_Matrix C = new B_Matrix(A.NRow, B.NCol);
			for (int i = 0; i < A.NRow; i++)
				for (int j = 0; j < B.NCol; j++)
				{
					for (int k = 0; k < A.NCol; k++)
						C[i, j] += A[i, k] * B[k, j];
				}
			return C;
		}
		public static B_Matrix operator *(B_Matrix A, double x)
		{
			B_Matrix B = new B_Matrix(A.NRow, A.NCol);
			for (int i = 0; i < A.NRow; i++)
				for (int j = 0; j < A.NCol; j++)
					B[i, j] = A[i, j] * x;
			return B;
		}
		public static B_Matrix operator *(double x, B_Matrix A)
		{
			return A * x;
		}
		public static B_Matrix operator /(B_Matrix A, double x)
		{
			return (1 / x) * A;
		}
		public static B_Matrix operator ^(B_Matrix A, int n)
		{
			B_Matrix Res = A.Clone();
			for (int i = 0; i < n - 1; i++)
				Res *= A;
			return Res;
		}

		public static B_Matrix Rotation3D(V3D a, double angle)
		{
			a = a.Unit;
			B_Matrix Res = new B_Matrix(4);
			double c = Math.Cos(angle / 180 * Math.PI);
			double s = Math.Sin(angle / 180 * Math.PI);
			Res[0, 0] = c + (a.X * a.X) * (1 - c);
			Res[0, 1] = a.X * a.Y * (1 - c) - a.Z * s;
			Res[0, 2] = a.X * a.Z * (1 - c) + a.Y * s;
			Res[0, 3] = 0;
			Res[1, 0] = a.X * a.Y * (1 - c) + a.Z * s;
			Res[1, 1] = c + a.Y * a.Y * (1 - c);
			Res[1, 2] = a.Y * a.Z * (1 - c) - a.X * s;
			Res[1, 3] = 0;
			Res[2, 0] = a.X * a.Z * (1 - c) - a.Y * s;
			Res[2, 1] = a.Y * a.Z * (1 - c) + a.X * s;
			Res[2, 2] = c + a.Z * a.Z * (1 - c);
			Res[2, 3] = 0;
			Res[3, 0] = Res[3, 1] = Res[3, 2] = 0;
			Res[3, 3] = 1;
			return Res;
		}
		public static B_Matrix Translation(V3D p)
		{
			B_Matrix Res = new B_Matrix(4);
			for (int i = 0; i < 4; i++)
				Res[i, i] = 1;
			Res[0, 3] = p.X;
			Res[1, 3] = p.Y;
			Res[2, 3] = p.Z;
			return Res;
		}

		public void Debug()
		{
			for (int i = 0; i < NRow; i++)
			{
				for (int j = 0; j < NCol; j++)
				{
					Console.Write(this[i, j].ToString() + " ");
				}
				Console.WriteLine();
			}
		}
	}
}